#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <helper/helper_utils.h>

// 运行示例： rosrun helper helper_odom_info /odom

/**
 * @description: define class OdomInfo
 * @return 
 */

class OdomInfo{
    public:
        OdomInfo();
        OdomInfo(string odom_topic);
        ~OdomInfo();


    private:
        ros::NodeHandle __nh;
        ros::Subscriber __odomSub;

        //里程计回调函数
        void OdomCallback(const nav_msgs::OdometryConstPtr& msg);

};


/**
 * @description: 
 * @return {*}
 */
OdomInfo::OdomInfo(){
    __odomSub = __nh.subscribe("/odom", 1, &OdomInfo::OdomCallback, this);
}

OdomInfo::OdomInfo(string odom_topic){
    __odomSub = __nh.subscribe(odom_topic, 1, &OdomInfo::OdomCallback, this);
}

/**
 * @description: 
 * @return {*}
 */
OdomInfo::~OdomInfo(){

}


/**
 * @description: 
 * @param {OdometryConstPtr&} msg
 * @return {*}
 */
void OdomInfo::OdomCallback(const nav_msgs::OdometryConstPtr& msg){
    // 打印里程计数据
    ROS_INFO("----------------------[OdomCallback]----------------------");
    Eigen::Vector3d euler = helper::quaternionToEuler(msg->pose.pose.orientation);
    ROS_INFO("[%s] position: (%.3f, %.3f, %.3f), euler: (%.3f, %.3f, %.3f), deg: (%.3f, %.3f, %.3f), twist: (%.3f, %.3f, %.3f).",
        helper::stamp_to_standard(msg->header.stamp.sec).c_str(),
        msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z,
        euler(0), euler(1), euler(2),
        helper::Rad2Deg(euler(0)), helper::Rad2Deg(euler(1)), helper::Rad2Deg(euler(2)),
        msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.angular.z);
}


/**
 * @description: 
 * @param {int} argc
 * @param {char} *
 * @return {*}
 */
int main(int argc, char **argv ){

    // 打印参数
    for(int i=0; i<argc; i++){
        std::cout << "i: " << i << ", data: " << argv[i] << std::endl;
    }
    if(argc < 2){
        std::cout << "please set odom topic."  << std::endl;
        return 1;
    }
    ros::init(argc, argv, "helper_odom_info");
    ROS_INFO("helper_odom_info initialization ......");
    ros::NodeHandle nh;
    OdomInfo odom_info(argv[1]);
    ros::spin();
    return 0;
}


